//
// Created by l on 2021/11/5.
//
#ifndef UAV_LASER_H
#define UAV_LASER_H


#include"RMap.h"
#include <cmath>
#include<algorithm>
#include<iostream>
#include<unordered_set>

using namespace std;
#define  MAX(a, b) (((a)>(b))?(a):(b))
#define  MIN(a, b) (((a)<(b))?(a):(b))

class Laser {
private:
    int resolution;//angle period
    double distance;//max distance for laser can detect
public:
    Laser(int resolution, double distance) {
        this->resolution = resolution;
        this->distance = distance;
    }

    vector<point> scan(RMap rmap, int, int) const;

    vector<point> scan_simple(RMap rmap, int, int) const;

};


vector<point> Laser::scan(RMap rmap, int sx, int sy) const {
    vector<point> res;
    for (int i = 0; i <= 360; i += resolution) {

        double radian = (double) i * 3.1415926 / 180.0;
        double vc_x = cos(radian);
        double vc_y = sin(radian);

        for (double j = 0;; j += 0.1) {
            double a = j * vc_x * j * vc_x;
            double b = j * vc_y * j * vc_y;
            if (a + b >= distance * distance) {
                break;
            }

            double next_x = (double) sx + j * vc_x;
            double next_y = (double) sy + j * vc_y;

            if (next_x >= rmap.m_line || next_y >= rmap.m_col || next_x < 0 || next_y < 0) {
                break;
            }

            if (rmap.m_map[(int) next_x][(int) next_y] == RMap::RMAP_OBSTACLE_UNDESCOVERED ||
                rmap.m_map[(int) next_x][(int) next_y] == RMap::RMAP_OBSTACLE_SCANNED) {

                point temp = point((int) next_x, (int) next_y);
//                if (temp.getx() == 31 && temp.gety() == 15) {
//                    cout << "next_x" << next_x << " " << "next_y" << next_y << endl;
//                    cout << "(int)next_x" << (int) next_x << " " << "(int)next_y" << (int) next_y << endl;
//                    double a = j * vc_x * j * vc_x;
//                    double b = j * vc_y * j * vc_y;
//                    cout << "length x" << j * vc_x * j * vc_x << endl;
//                    cout << "length y" << j * vc_y * j * vc_y << endl;
//                    cout << "a+b" << a + b << endl;
//                    cout << "length " << (j * vc_x * j * vc_x) + (j * vc_y + j * vc_y) << endl;
//                    cout << "i " << i << " j" << j << endl;
//                    while (1) {}
//
//                }
                res.push_back(temp);
                break;
            }

        }

    }
    sort(res.begin(), res.end());
    res.erase(unique(res.begin(), res.end()), res.end());

    return res;
}

vector<point> Laser::scan_simple(RMap rmap, int sx, int sy) const {
    vector<point> res;
    int ux = MAX(sx - (int) distance, 0);//最up边
    int dx = MIN(sx + (int) distance, 99);

    int ly = MAX(sy - (int) distance, 0);//
    int ry = MIN(sy + (int) distance, 99);//
    cout << "x范围 " << ux << dx << endl;
    cout << "y范围 " << ly << ry << endl;

    for (int x = ux; x <= dx; x++) {
        for (int y = ly; y <= ry; y++) {
            cout << "扫到的点 " << x << " " << y << endl;
            if (rmap.m_map[(int) x][(int) y] == RMap::RMAP_OBSTACLE_UNDESCOVERED ||
                rmap.m_map[(int) x][(int) y] == RMap::RMAP_OBSTACLE_SCANNED) {
                res.emplace_back(point(x, y));

            }
        }
    }


    return res;
}

#endif //UAV_LASER_H





